/******************** (C) COPYRIGHT 2017 ANO Tech ********************************
 * 作者    ：匿名科创
 * 官网    ：www.anotc.com
 * 淘宝    ：anotc.taobao.com
 * 技术Q群 ：190169595
 * 描述    ：数据传输
 **********************************************************************************/
/*============================================================================
更新：
20201119：更新V7版协议
===========================================================================*/

#include "Ano_DT.h"
#include "include.h"
#include "Ano_USB.h"
#include "Drv_usart.h"
#include "Ano_Parameter.h"
#include "Ano_Imu.h"
#include "Ano_Imu_Data.h"
#include "Ano_Imu_Calibration.h"
#include "Ano_MagProcess.h"
#include "Ano_Power.h"
#include "Ano_MotionCal.h"
#include "Ano_FlightCtrl.h"
#include "Ano_RC.h"
#include "Ano_LocCtrl.h"
#include "Drv_spl06.h"
#include "Ano_OF.h"
#include "Drv_led.h"
#include "Ano_ProgramCtrl_User.h"
/*============================================================================
******************************************************************************
******************************************************************************
基础定义部分，定义数据传输使用到的基本元素，用户使用及二次开发无需修改本部分。
******************************************************************************
******************************************************************************
============================================================================*/
#define BYTE0(dwTemp) (*((char *)(&dwTemp)))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

#define DT_RX_BUFNUM 64
#define DT_ODNUM 5 // 非循环发送数据缓冲大小
#define DT_SENDPTR_HID Usb_Hid_Adddata
#define DT_SENDPTR_U2 Uart5_Send

// 越往前发送优先级越高，如果需要修改，这里和h文件里的枚举需要同时改
const u8 _cs_idlist[CSID_NUM] = {0x20, 0x21, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0f, 0x30, 0x32, 0x33, 0x34, 0x40, 0x41, 0xFA};
// 循环发送数据结构体
typedef struct
{
	u8 WTS;			 // wait to send等待发送标记
	u16 fre_ms;		 // 发送周期
	u16 time_cnt_ms; // 计时变量
} _dt_frame_st;
typedef struct
{
	_dt_frame_st txSet_u1[CSID_NUM];
	_dt_frame_st txSet_u2[CSID_NUM];
	_dt_frame_st txSet_usb[CSID_NUM];
} _dt_st;
_dt_st dt;
u8 CycleSendData[64]; // 循环发送数据临时缓冲
// 非循环发送数据结构体
typedef struct
{
	u8 WTS;
	u8 type;
	u8 len;
	u8 data[64];
} _dt_otherdata_st;
_dt_otherdata_st OtherSendData[DT_ODNUM];
u8 otherDataTmp[64]; // 非循环发送数据临时缓冲

/*============================================================================
******************************************************************************
******************************************************************************
数据接口，本部分通过宏定义将各种需要收发的数据进行定义，普通用户只需修改本部分
定义即可实现不同数据的收发功能
******************************************************************************
******************************************************************************
============================================================================*/
// 0x01
#define ACC_RAW_X (st_imuData.f_acc_cmpss_nb[0])
#define ACC_RAW_Y (st_imuData.f_acc_cmpss_nb[1])
#define ACC_RAW_Z (st_imuData.f_acc_cmpss_nb[2])
#define GYR_RAW_X (st_imuData.f_gyr_dps_nb[0])
#define GYR_RAW_Y (st_imuData.f_gyr_dps_nb[1])
#define GYR_RAW_Z (st_imuData.f_gyr_dps_nb[2])
#define SHOCK_STA (st_imuData.data_sta)
// 0x02
#define ECP_RAW_X (mag.val[0])
#define ECP_RAW_Y (mag.val[1])
#define ECP_RAW_Z (mag.val[2])
#define BARO_ALT (spl_data.height)
#define TEMPERATURE (st_imuData.f_temperature)
#define BARO_STA (0)
#define ECP_STA (0)
// 0x03
#define ANGLE_ROL (imu_data.rol)
#define ANGLE_PIT (imu_data.pit)
#define ANGLE_YAW (imu_data.yaw)
#define ATT_FUSION_STA (0)
// 0x04
// #define QUA0_10K       (imu_att.qua.w *1e4f)
// #define QUA1_10K       (imu_att.qua.x *1e4f)
// #define QUA2_10K       (imu_att.qua.y *1e4f)
// #define QUA3_10K       (imu_att.qua.z *1e4f)
// 0X05
#define ALT_FU (wcz_hei_fus.out)
#define ALT_ADD (jsdata.of_alt)
#define ALT_STA (0)
// 0X06
#define FC_MODE (flag.flight_mode)
#define FC_LOCKED (flag.unlock_sta)
#define FC_CMD_ID (0)
#define FC_CMD_0 (0)
#define FC_CMD_1 (0)
// 0X07
#define HCA_VEL_X (loc_ctrl_1.fb[0])
#define HCA_VEL_Y (loc_ctrl_1.fb[1])
#define HCA_VEL_Z (wcz_spe_fus.out)
////0x08
#define ULHCA_POS_X (0)
#define ULHCA_POS_Y (0)
////0X09
// #define HCA_WIND_X      (nowind_est.wind_vel_h[X])
// #define HCA_WIND_Y      (nowind_est.wind_vel_h[Y])
////0X0A
// #define TAR_ROL         (0)
// #define TAR_PIT         (0)
// #define TAR_YAW         (0)
// 0X0B
#define HCA_TAR_VEL_X (fc_in.tar_vel_cmps_h[X])
#define HCA_TAR_VEL_Y (fc_in.tar_vel_cmps_h[Y])
#define HCA_TAR_VEL_Z (fc_in.tar_vel_cmps_h[Z])
// 0X0D
#define BAT_VOLTAGE_100 (Plane_Votage * 100)
////0x20
// #define PWM_1          (mt_ct_val.moto_pwm_u16[0])
// #define PWM_2          (mt_ct_val.moto_pwm_u16[1])
// #define PWM_3          (mt_ct_val.moto_pwm_u16[2])
// #define PWM_4          (mt_ct_val.moto_pwm_u16[3])
// #define PWM_5          (0)//
// #define PWM_6          (0)
// #define PWM_7          (0)
// #define PWM_8          (0)
////0x21
// #define CTRL_ROL	(ctrl_val.rol_ctrl_val)
// #define CTRL_PIT	(ctrl_val.pit_ctrl_val)
// #define CTRL_THR	(ctrl_val.thr_ctrl_val)
// #define CTRL_YAW	(ctrl_val.yaw_ctrl_val)

/*======================================================================================================================
//数据发送初始化
======================================================================================================================*/
void ANO_DT_Init(void)
{
	// 串口1发送配置/////////////////////////////////////////////////////////////
	//	//PWM
	//	dt.txSet_u2[CSID_X20].fre_ms = 0;  //外部触发
	// ACC-GRO
	dt.txSet_u2[CSID_X01].fre_ms = 10; // 10ms
	// ECP-TEM-BARO
	dt.txSet_u2[CSID_X02].fre_ms = 20; // 20ms
	// ATT_ANG
	dt.txSet_u2[CSID_X03].fre_ms = 10; // ms
	// ATT_QUA
	dt.txSet_u2[CSID_X04].fre_ms = 0; //
	// height
	dt.txSet_u2[CSID_X05].fre_ms = 50; //
	// fc_mode
	dt.txSet_u2[CSID_X06].fre_ms = 50; //
	// velocity
	dt.txSet_u2[CSID_X07].fre_ms = 50; //
	// pos
	dt.txSet_u2[CSID_X08].fre_ms = 50; //
									   //	//wind_vel
									   //	dt.txSet_u2[CSID_X09].fre_ms = 100;//
	// 电压
	dt.txSet_u2[CSID_X0D].fre_ms = 200; //
	// 传感器状态
	dt.txSet_u2[CSID_X0E].fre_ms = 200; //
	// GPS数据
	dt.txSet_u2[CSID_X30].fre_ms = 200; //
	// 遥控数据
	dt.txSet_u2[CSID_X40].fre_ms = 100; //
										//	//实时控制数据
										//	dt.txSet_u2[CSID_X41].fre_ms = 50;//
										//	//FC_RGB
										//	dt.txSet_u2[CSID_X0F].fre_ms = 0;//
										//
#ifdef DEBUG
	//
	//	dt.txSet_usb[CSID_X40].fre_ms = 1;
	dt.txSet_u2[CSID_XFA].fre_ms = 2;
#endif
	// USB发送配置///////////////////////////////////////////////////////////////
	for (u8 i = 0; i < CSID_NUM; i++)
	{
		dt.txSet_usb[i].fre_ms = dt.txSet_u2[i].fre_ms;
		dt.txSet_usb[i].time_cnt_ms = dt.txSet_u2[i].time_cnt_ms;
	}
}

/*============================================================================
******************************************************************************
******************************************************************************
数据发送相关函数
******************************************************************************
******************************************************************************
============================================================================*/
//===========================================================
// 此函数是协议中所有发送数据功能使用到的发送函数
// 移植时，用户应根据自身应用的情况，根据使用的通信方式，实现此函数
//===========================================================
void ANO_DT_Send_Data(u8 type, u8 *dataToSend, u8 length)
{
	if ((type & USE_HID)) // && fc_sta.unlock_sta == 0)
		DT_SENDPTR_HID(dataToSend, length);
	if (type & USE_U2)
		DT_SENDPTR_U2(dataToSend, length);
}
//===========================================================
// 根据ID填充数据帧的data区域
//===========================================================
static void DTFrameAddData(u8 frame_num, u8 *_cnt)
{
	s16 temp_data;
	s32 temp_data_32;
	switch (frame_num)
	{
	case 0x01:
	{
		// ACC GYR
		temp_data = (s16)(ACC_RAW_X);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ACC_RAW_Y);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ACC_RAW_Z);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(GYR_RAW_X);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(GYR_RAW_Y);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(GYR_RAW_Z);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		// STATE
		CycleSendData[(*_cnt)++] = SHOCK_STA;
	}
	break;
	case 0x02:
	{
		// ECP
		temp_data = (s16)(ECP_RAW_X);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ECP_RAW_Y);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ECP_RAW_Z);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		// BARO_ALT
		temp_data_32 = (s32)(BARO_ALT);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE2(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE3(temp_data_32);
		// temperature
		temp_data = (s16)(TEMPERATURE);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		// BARO_STA
		CycleSendData[(*_cnt)++] = BARO_STA;
		// ECP_STA
		CycleSendData[(*_cnt)++] = ECP_STA;
	}
	break;
	case 0x03:
	{
		// ATT_angle
		temp_data = (s16)(ANGLE_ROL * 100);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ANGLE_PIT * 100);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(ANGLE_YAW * 100);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		// STATE
		CycleSendData[(*_cnt)++] = ATT_FUSION_STA;
	}
	break;
	case 0x04:
	{
	}
	break;
	case 0x05:
	{
		// HEIGHT
		temp_data_32 = (s32)(ALT_FU);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE2(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE3(temp_data_32);
		temp_data_32 = (s32)(ALT_ADD);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE2(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE3(temp_data_32);
		//
		CycleSendData[(*_cnt)++] = ALT_STA;
	}
	break;
	case 0x06:
	{
		// MODE	LOCKED	FUN	CMD
		CycleSendData[(*_cnt)++] = FC_MODE;
		CycleSendData[(*_cnt)++] = FC_LOCKED;
		CycleSendData[(*_cnt)++] = FC_CMD_ID;
		CycleSendData[(*_cnt)++] = FC_CMD_0;
		CycleSendData[(*_cnt)++] = FC_CMD_1;
	}
	break;
	case 0x07:
	{
		// velocity
		temp_data = (s16)(HCA_VEL_X);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(HCA_VEL_Y);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		temp_data = (s16)(HCA_VEL_Z);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
	}
	break;
	case 0x08:
	{
		// pos
		temp_data_32 = (s32)(ULHCA_POS_X);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE2(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE3(temp_data_32);
		temp_data_32 = (s32)(ULHCA_POS_Y);
		CycleSendData[(*_cnt)++] = BYTE0(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE2(temp_data_32);
		CycleSendData[(*_cnt)++] = BYTE3(temp_data_32);
	}
	break;
	case 0x09:
	{
	}
	break;
	case 0x0D:
	{
		// 电压
		temp_data = (s16)BAT_VOLTAGE_100;
		CycleSendData[(*_cnt)++] = BYTE0(temp_data);
		CycleSendData[(*_cnt)++] = BYTE1(temp_data);
		CycleSendData[(*_cnt)++] = 0;
		CycleSendData[(*_cnt)++] = 0;
	}
	break;
	case 0x0E:
	{
		// 传感器状态
		CycleSendData[(*_cnt)++] = switchs.of_flow_on;
		CycleSendData[(*_cnt)++] = 0;
		CycleSendData[(*_cnt)++] = switchs.gps_on;
		CycleSendData[(*_cnt)++] = switchs.of_tof_on;
	}
	break;
	case 0x30:
	{
		//
		for (u8 i = 0; i < 23; i++)
		{
			CycleSendData[(*_cnt)++] = ext_sens.fc_gps.byte[i];
		}
	}
	break;
	case 0x40:
	{
		for (u8 i = 0; i < 20; i++)
		{
			CycleSendData[(*_cnt)++] = rc_in.rc_ch.byte_data[i];
		}
	}
	break;
	case 0x41:
	{
	}
	break;
	case 0x0f: // FC_RGB
	{
	}
	break;
	case 0x20:
	{
	}
	break;
	case 0xfa:
	{
	}
	break;
	default:
		break;
	}
}

//===========================================================
// 根据ID组织发送数据帧
//===========================================================
static void DTFrameSend(u8 type, u8 id_addr, u8 addr)
{
	u8 _cnt = 0;
	u8 _id;
	vs16 _temp;

	CycleSendData[_cnt++] = FRAME_HEAD;
	CycleSendData[_cnt++] = addr;
	if (id_addr >= CSID_NUM)
		return;
	_id = _cs_idlist[id_addr];
	CycleSendData[_cnt++] = _id;
	CycleSendData[_cnt++] = 0;
	//==
	DTFrameAddData(_id, &_cnt);
	//==
	CycleSendData[3] = _cnt - 4;
	//==
	u8 check_sum1 = 0, check_sum2 = 0;
	for (u8 i = 0; i < _cnt; i++)
	{
		check_sum1 += CycleSendData[i];
		check_sum2 += check_sum1;
	}
	CycleSendData[_cnt++] = check_sum1;
	CycleSendData[_cnt++] = check_sum2;

	ANO_DT_Send_Data(type, CycleSendData, _cnt);
}

//===========================================================
// 非循环发送数据的写入缓冲区
//===========================================================
static u8 OtherSendDataAdd(u8 _type, u8 *_data, u8 _len)
{
	for (u8 i = 0; i < DT_ODNUM; i++)
	{
		if (OtherSendData[i].WTS == 0)
		{
			OtherSendData[i].type = _type;
			OtherSendData[i].len = _len;
			for (u8 j = 0; j < _len; j++)
				OtherSendData[i].data[j] = *(_data + j);
			OtherSendData[i].WTS = 1;
			return 1;
		}
	}
	return 0;
}
//===========================================================
// 判断是否有需要发送的非循环数据，如有，则启动发送
//===========================================================
static void OtherSendDataCheck(void)
{
	for (u8 i = 0; i < DT_ODNUM; i++)
	{
		if (OtherSendData[i].WTS)
		{
			ANO_DT_Send_Data(OtherSendData[i].type, OtherSendData[i].data, OtherSendData[i].len);
			OtherSendData[i].WTS = 0;
			return;
		}
	}
}
//===========================================================
// 检查循环发送的数据有没有到指定的时间间隔
//===========================================================
static void CheckDotMs(u8 type, u8 id_addr)
{
	u16 *_dot_ms;
	u16 *_dot_cnt_ms;
	u8 *_dot_WTS;

	switch (type)
	{
	case USE_HID:
		_dot_ms = &dt.txSet_usb[id_addr].fre_ms;
		_dot_cnt_ms = &dt.txSet_usb[id_addr].time_cnt_ms;
		_dot_WTS = &dt.txSet_usb[id_addr].WTS;
		break;
	case USE_U1:
		_dot_ms = &dt.txSet_u1[id_addr].fre_ms;
		_dot_cnt_ms = &dt.txSet_u1[id_addr].time_cnt_ms;
		_dot_WTS = &dt.txSet_u1[id_addr].WTS;
		break;
	case USE_U2:
		_dot_ms = &dt.txSet_u2[id_addr].fre_ms;
		_dot_cnt_ms = &dt.txSet_u2[id_addr].time_cnt_ms;
		_dot_WTS = &dt.txSet_u2[id_addr].WTS;
		break;
	default:
		return;
	}
	if (id_addr >= CSID_NUM)
		return;
	if ((*_dot_ms) != 0)
	{
		if ((*_dot_cnt_ms) < (*_dot_ms))
		{
			(*_dot_cnt_ms)++;
		}
		else
		{
			// 清除计时并置位等待发送标记
			(*_dot_WTS) = 1;
			(*_dot_cnt_ms) = 1;
		}
	}
}
//===========================================================
// 检查有没有需要循环发送的数据
//===========================================================
u8 CheckDotWts(u8 type, u8 id_addr)
{
	u8 _addr;
	u8 *_dot_WTS;
	switch (type)
	{
	case USE_HID:
		_addr = 0xaf;
		_dot_WTS = &dt.txSet_usb[id_addr].WTS;
		break;
	case USE_U1:
		_addr = 0xff;
		_dot_WTS = &dt.txSet_u1[id_addr].WTS;
		break;
	case USE_U2:
		_addr = 0xaf; // swj
		_dot_WTS = &dt.txSet_u2[id_addr].WTS;
		break;
	default:
		return 0;
	}
	if (id_addr >= CSID_NUM)
	{
		return 0;
	}
	if ((*_dot_WTS))
	{
		// 复位等待发送标记
		(*_dot_WTS) = 0;
		// 实际发送
		DTFrameSend(type, id_addr, _addr);
		return 1;
	}
	else
	{
		return 0;
	}
}

//===========================================================
// 发送Check帧，见协议文本
//===========================================================
static void SendCheck(u8 type, u8 dest_addr, u8 cid, u8 sc, u8 ac)
{
	u8 _cnt = 0;

	otherDataTmp[_cnt++] = FRAME_HEAD;
	otherDataTmp[_cnt++] = dest_addr;
	otherDataTmp[_cnt++] = 0;
	otherDataTmp[_cnt++] = 0;

	otherDataTmp[_cnt++] = cid;
	otherDataTmp[_cnt++] = sc;
	otherDataTmp[_cnt++] = ac;

	otherDataTmp[3] = _cnt - 4;
	u8 check_sum1 = 0, check_sum2 = 0;
	for (u8 i = 0; i < _cnt; i++)
	{
		check_sum1 += otherDataTmp[i];
		check_sum2 += check_sum1;
	}
	otherDataTmp[_cnt++] = check_sum1;
	otherDataTmp[_cnt++] = check_sum2;

	OtherSendDataAdd(type, otherDataTmp, _cnt);
}
//===========================================================
// 发送参数
//===========================================================
static void SendPar(u8 type, u8 dest_addr, u16 p_id)
{
	s32 p_val = 0;
	p_val = AnoParRead(p_id);

	u8 _cnt = 0;

	otherDataTmp[_cnt++] = FRAME_HEAD;
	otherDataTmp[_cnt++] = dest_addr;
	otherDataTmp[_cnt++] = 0xE2;
	otherDataTmp[_cnt++] = 0;

	otherDataTmp[_cnt++] = BYTE0(p_id);
	otherDataTmp[_cnt++] = BYTE1(p_id);

	otherDataTmp[_cnt++] = BYTE0(p_val);
	otherDataTmp[_cnt++] = BYTE1(p_val);
	otherDataTmp[_cnt++] = BYTE2(p_val);
	otherDataTmp[_cnt++] = BYTE3(p_val);

	otherDataTmp[3] = _cnt - 4;
	u8 check_sum1 = 0, check_sum2 = 0;
	for (u8 i = 0; i < _cnt; i++)
	{
		check_sum1 += otherDataTmp[i];
		check_sum2 += check_sum1;
	}
	otherDataTmp[_cnt++] = check_sum1;
	otherDataTmp[_cnt++] = check_sum2;

	OtherSendDataAdd(type, otherDataTmp, _cnt);
}
//===========================================================
// 接收到参数后启动参数保存
//===========================================================
static void WritePar(u16 p_id, s32 p_val)
{
	switch (p_id)
	{

	default:
		p_val = p_val;
		break;
	}

	// 触发延时写入
	// Data_Save_Trig();
}
//===========================================================
// 发送字符串给上位机log区域显示
//===========================================================
void AnoDTSendStr(u8 type, u8 dest_addr, u8 string_color, char *str)
{
	u8 _cnt = 0;

	otherDataTmp[_cnt++] = FRAME_HEAD;
	otherDataTmp[_cnt++] = dest_addr;
	otherDataTmp[_cnt++] = 0xA0;
	otherDataTmp[_cnt++] = 0;

	otherDataTmp[_cnt++] = string_color;
	u8 i = 0;
	while (*(str + i) != '\0') // 单引号字符，双引号字符串
	{
		otherDataTmp[_cnt++] = *(str + i++);
		if (_cnt > 50)
		{
			break;
		}
	}

	otherDataTmp[3] = _cnt - 4;
	u8 check_sum1 = 0, check_sum2 = 0;
	for (u8 i = 0; i < _cnt; i++)
	{
		check_sum1 += otherDataTmp[i];
		check_sum2 += check_sum1;
	}
	otherDataTmp[_cnt++] = check_sum1;
	otherDataTmp[_cnt++] = check_sum2;

	OtherSendDataAdd(type, otherDataTmp, _cnt);
}

/*============================================================================
******************************************************************************
******************************************************************************
数据接收相关函数
******************************************************************************
******************************************************************************
============================================================================*/
static void AnoDTDataAnl(u8 type, u8 *data, u8 len);
//===========================================================
// 根据不同通信方式，定义接收数据缓冲区
//===========================================================
typedef struct
{
	u8 DT_RxBuffer[DT_RX_BUFNUM];
	u8 _data_len;
	u8 _data_cnt;
	u8 rxstate;
} _dt_rx_anl_st;
//===========================================================
// 数据每接收一字节，调用本函数，进行数据解析
//===========================================================
void AnoDTRxOneByte(u8 type, u8 data)
{
	static _dt_rx_anl_st rx_anl[3];

	switch (rx_anl[type].rxstate)
	{
	case 0:
	{
		if (data == 0xAA)
		{
			rx_anl[type].rxstate = 1;
			rx_anl[type].DT_RxBuffer[0] = data;
		}
	}
	break;
	case 1:
	{
		if ((data == HW_TYPE || data == HW_ALL || data == 0x05))
		{
			rx_anl[type].rxstate = 2;
			rx_anl[type].DT_RxBuffer[1] = data;
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 2:
	{
		rx_anl[type].rxstate = 3;
		rx_anl[type].DT_RxBuffer[2] = data;
	}
	break;
	case 3:
	{
		if (data < (DT_RX_BUFNUM - 5))
		{
			rx_anl[type].rxstate = 4;
			rx_anl[type].DT_RxBuffer[3] = data;
			rx_anl[type]._data_len = data;
			rx_anl[type]._data_cnt = 0;
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 4:
	{
		if (rx_anl[type]._data_len > 0)
		{
			rx_anl[type]._data_len--;
			rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt++] = data;
			if (rx_anl[type]._data_len == 0)
			{
				rx_anl[type].rxstate = 5;
			}
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 5:
	{
		rx_anl[type].rxstate = 6;
		rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt++] = data;
	}
	break;
	case 6:
	{
		u8 _data_cnt;
		rx_anl[type].rxstate = 0;
		rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt] = data;
		_data_cnt = rx_anl[type]._data_cnt + 5;
		// ano_dt_data_ok = 1;
		AnoDTDataAnl(type, rx_anl[type].DT_RxBuffer, _data_cnt);
	}
	break;
	default:
	{
		rx_anl[type].rxstate = 0;
	}
	break;
	}
}
void AnoDTRxOneByteUart(u8 data)
{
	AnoDTRxOneByte(USE_U2, data);
}
void AnoDTRxOneByteUsb(u8 data)
{
	AnoDTRxOneByte(USE_HID, data);
}

//===========================================================
// AnoDTDataAnl函数是协议数据解析函数，函数参数是符合协议格式的一个数据帧，该函数会首先对协议数据进行校验
// 此函数可以不用用户自行调用，由函数Data_Receive_Prepare自动调用
//===========================================================
static void AnoDTDataAnl(u8 type, u8 *data, u8 len)
{
	u8 check_sum1 = 0, check_sum2 = 0;
	if (*(data + 3) != (len - 6)) // 判断数据长度是否正确
	{
		return;
	}
	for (u8 i = 0; i < len - 2; i++)
	{
		check_sum1 += *(data + i);
		check_sum2 += check_sum1;
	}
	if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) // 判断sum校验
	{
		return;
	}
	//=============================================================================
	if (*(data + 2) == 0X40) // 摇杆数据
	{
	}
	else if (*(data + 2) == 0X41) // 实时控制数据
	{
	}
	else if (*(data + 2) == 0X30) // GPS数据
	{
	}
	else if (*(data + 2) == 0X32) // 通用位置
	{
	}
	else if (*(data + 2) == 0X33) // 通用速度
	{
	}
	else if (*(data + 2) == 0X34) // 通用测距
	{
	}
	else if (*(data + 2) == 0X0D) // 电压
	{
	}
	else if (*(data + 2) == 0XE0) // 命令E0
	{
		switch (*(data + 4)) // CID
		{
		case 0x01:
		{
			if (*(data + 5) == 0x00)
			{
				if (*(data + 6) == 0x01) // acc
				{
				}
				else if (*(data + 6) == 0x02) // gyro
				{
				}
				else if (*(data + 6) == 0x03) // horizontal
				{
				}
				else if (*(data + 6) == 0x04) // ecp
				{
				}
				else if (*(data + 6) == 0x05) // 6 side
				{
				}
				else if (*(data + 6) == 0x10) // imu_reset
				{
				}
				else if (*(data + 6) == 0x61) // 存储航点
				{
				}
				else if (*(data + 6) == 0x62) // 清空航点
				{
				}
				else if (*(data + 6) == 0xAA) // 恢复默认PID
				{
					PID_Rest();
					data_save();
				}
				else if (*(data + 6) == 0xAB) // 恢复默认参数
				{
					Parame_Reset(1);
					data_save();
				}
				else if (*(data + 6) == 0xAC) // 恢复所有参数
				{
					PID_Rest();
					Parame_Reset(2);
					data_save();
				}
			}
			else if (*(data + 5) == 0x01)
			{
				if (*(data + 6) == 0x01) // 飞控模式
				{
				}
			}
			else if (*(data + 5) == 0x30) // 校准exe
			{
				// 校准命令
				if (*(data + 6) == 0x01) //
				{
					if (flag.unlock_sta == 0)
					{
						// 开始6面校准
						st_imu_cali.acc_cali_on = 1;
						Ano_Parame.set.acc_calibrated = 0;
					}
				}
				else if (*(data + 6) == 0x02)
				{
					if (flag.unlock_sta == 0)
					{
						// 开始mag校准
						mag.mag_CALIBRATE = 2;
						Ano_Parame.set.mag_calibrated = 0;
					}
				}
			}
		}
		break;
		case 0x10: // 指令控制
		{
			if (*(data + 5) == 0x00)
			{
				switch (*(data + 6))
				{
				case 0x01: // 解锁
				{
				}
				break;
				case 0x02: // 上锁
				{
				}
				break;
				case 0x03:
				{
				}
				break;
				case 0x04: // 立即悬停
				{
				}
				break;
				case 0x05: // 起飞，带高度参数
				{
				}
				break;
				case 0x06: // 降落
				{
				}
				break;
				case 0x07: // 返航
				{
				}
				break;
				case 0x08: // 翻滚
				{
				}
				break;
				case 0x09: // 环绕
				{
				}
				break;
				case 0x0a: // 无头
				{
				}
				break;
				case 0x60: // 开始航点
				{
				}
				break;
				case 0x61: // 暂停航点
				{
				}
				break;
				case 0x62: // 取消航点
				{
				}
				break;
				default:
					break;
				}
			}
			else if (*(data + 5) == 0x01)
			{
			}
			else if (*(data + 5) == 0x02)
			{
			}
			else if (*(data + 5) == 0x03)
			{
			}
		}
		break;
		case 0x11:
			break;
		default:
			break;
		}
		// 需返回CHECK帧
		SendCheck(type, SWJ_ADDR, *(data + 2), check_sum1, check_sum2);
	}
	else if (*(data + 2) == 0X00) // Check
	{
	}
	else if (*(data + 2) == 0XE1)
	{
		// 读取参数
		SendPar(type, SWJ_ADDR, *(u16 *)(data + 4));
	}
	else if (*(data + 2) == 0xE2)
	{
		// 写入参数
		AnoParWrite(*(u16 *)(data + 4), *(s32 *)(data + 6));
		// 写入参数后需返回CHECK帧，告诉上位机参数写入成功
		SendCheck(type, SWJ_ADDR, *(data + 2), check_sum1, check_sum2);
	}
	else if (*(data + 2) == 0x60)
	{
		// 读取航点
	}
	else if (*(data + 2) == 0x61)
	{
		// 写入航点
	}
}

data_float t265_posx, t265_posy, t265_posz;
data_int_ camera_black_x, camera_black_y, camera_red_x, camera_red_y;
float t265_x_lpf = 0.0f, t265_y_lpf = 0.0f, t265_z_lpf = 0.0f;
// float t265_x_dev = 0.0f, t265_y_dev = 0.0f, t265_z_dev = 0.0f;
u32 t265_dT_[3];
u8 fire_area = 0;
u16 receive_black_times = 0;

extern u32 black_time;
extern u32 t265_time;
extern int flight_vx, flight_vy;
extern int point_num;
extern int land_red_x, land_red_y;
extern int land_x, land_y;
// extern u32 _test_dT_1000hz[3];
//===========================================================
// 整体数据通信的调度器，需要1ms调用一次
//===========================================================
void ANO_DT_Task1Ms(void)
{
	// 检查有没有非循环发送的数据需要发送
	OtherSendDataCheck();
	// 检查循环发送的数据有没有需要发送的
	for (u8 i = 0; i < CSID_NUM; i++)
	{
		CheckDotMs(USE_U2, i);
		CheckDotMs(USE_HID, i);
		// CheckDotMs(USE_U1,i);
	}
	// 串口数据发送执行
	for (u8 i = 0; i < CSID_NUM; i++)
	{
		CheckDotWts(USE_U2, i);
	}
	// USB数据发送执行
	for (u8 i = 0; i < CSID_NUM; i++)
	{
		if (CheckDotWts(USE_HID, i))
			break;
	}
	// USB发送服务程序
	Usb_Hid_Send();

	// ANODT_UserDefined_t265((int32_t)(t265_of_x * 1000),
	//    (int32_t)(t265_of_y * 1000), 0);
	// ANODT_UserDefined_t265((int32_t)(t265_x_lpf * 1000),
	//    (int32_t)(-t265_x_dev * 1000), (int32_t)(t265_z_dev * 1000));
	//	ANODT_UserDefined_t265_lpf((int32_t)(t265_posy.data_f * 1000),
	//							   (int32_t)(-t265_posx.data_f * 1000), 0);
	// ANODT_UserDefined_flow(flag.fire_area, fire_area, camera_red_x.data_int_, camera_red_y.data_int_);
	ANODT_UserDefined_flow(land_red_x, fire_area, point_num_2, flag.find_fire_area);
}
/******************* (C) COPYRIGHT 2014 ANO TECH *****END OF FILE************/

static void MYDTDataAnl(u8 type, u8 *data, u8 len)
{
	u8 check_sum1 = 0, check_sum2 = 0;
	if (*(data + 3) != (len - 6)) // 判断数据长度是否正确
	{
		return;
	}
	for (u8 i = 0; i < len - 2; i++)
	{
		check_sum1 += *(data + i);
		check_sum2 += check_sum1;
	}
	if ((check_sum1 != *(data + len - 2)) || (check_sum2 != *(data + len - 1))) // 判断sum校验
	{
		return;
	}
	else if (*(data + 2) == 0Xf8) // 起飞命令
	{
		if (*(data + 4) == 1)
		{
			flag.car_to_plane_take_off = 1;
		}
	}
	else if (*(data + 2) == 0xf5) // 视觉
	{
		if (flag.prepare_camera_to_land) // 最后一公里、降落过程中才会触发
		{
			for (u8 i = 0; i < 2; i++)
			{
				camera_black_x.data_u[i] = *(data + 8 + i);
			}
			for (u8 i = 0; i < 2; i++)
			{
				camera_black_y.data_u[i] = *(data + 10 + i);
			}
			if (camera_black_x.data_int_ != 0 || camera_black_y.data_int_ != 0)
			{
				receive_black_times++;
			}
			if (black_time <= 200)
			{
				if (receive_black_times > 3)
					flag.camera_to_land = 1;
			}
		}
		/*红色坐标处理*/
		for (u8 i = 0; i < 2; i++)
		{
			camera_red_x.data_u[i] = *(data + 4 + i);
		}
		for (u8 i = 0; i < 2; i++)
		{
			camera_red_y.data_u[i] = *(data + 6 + i);
		}
		if (camera_red_x.data_int_ != 0 || camera_red_y.data_int_ != 0)
		{
			if (flag.find_fire_area)
			{
				// if ((abs(camera_red_x.data_int_ - 320) < 100) && abs(camera_red_y.data_int_ - 240) < 80)
				flag.fire_area = 1;
			}
			// if (point_num_2 == 0 && t265_of_x > 80)
			// {
			// 	if ((abs(camera_red_x.data_int_ - 320) < 100) && abs(camera_red_y.data_int_ - 240) < 80)
			// 		flag.fire_area = 1;
			// }
			// else if (point_num_2 >= 7)
			// {
			// 	flag.fire_area = 0;
			// }
			// else
			// {
			// 	if ((abs(camera_red_x.data_int_ - 320) < 100) && abs(camera_red_y.data_int_ - 240) < 80)
			// 		flag.fire_area = 1;
			// }
		}
	}
	else if (*(data + 2) == 0X32) // 通用位置
	{
		//		static float t265_dt = 0, last_t265_x, last_t265_y, last_t265_z;
		// t265_dT_[0] = t265_dT_[1];
		// t265_dT_[1] = GetSysTime_us();
		// t265_dT_[2] = (u32)(t265_dT_[1] - t265_dT_[0]);
		// t265_dt = t265_dT_[2];
		// t265_dt /= 1000000;

		/*t265数据有效*/
		LED_STA.t265_valid = 1;
		t265_time = 0;

		for (u8 i = 0; i < 4; i++)
		{
			t265_posx.data_u[i] = *(data + 4 + i);
		}
		for (u8 i = 0; i < 4; i++)
		{
			t265_posy.data_u[i] = *(data + 8 + i);
		}
		for (u8 i = 0; i < 4; i++)
		{
			t265_posz.data_u[i] = *(data + 12 + i);
		}

		static float alpha = 0.25f;
		t265_x_lpf += (t265_posx.data_f - t265_x_lpf) * alpha;
		t265_y_lpf += (t265_posy.data_f - t265_y_lpf) * alpha;
		t265_z_lpf += (t265_posz.data_f - t265_z_lpf) * alpha;

		/*差分算T265速度*/
		// t265_x_dev = (t265_posx.data_f - last_t265_x) / (t265_dt * 10);
		// t265_y_dev = (t265_posy.data_f - last_t265_y) / (t265_dt * 10);
		// t265_z_dev = (t265_posz.data_f - last_t265_z) / (t265_dt * 10);

		/*滤掉跳点*/
		// static float last_t265_x_dev, last_t265_y_dev, last_t265_z_dev;
		// if (my_abs(t265_x_dev - last_t265_x_dev) >= 0.05f)
		// {
		// 	t265_x_dev = last_t265_x_dev;
		// }
		// if (my_abs(t265_y_dev - last_t265_y_dev) >= 0.05f)
		// {
		// 	t265_y_dev = last_t265_y_dev;
		// }
		// if (my_abs(t265_z_dev - last_t265_z_dev) >= 0.05f)
		// {
		// 	t265_z_dev = last_t265_z_dev;
		// }
		// last_t265_x_dev = t265_x_dev;
		// last_t265_y_dev = t265_y_dev;
		// last_t265_z_dev = t265_z_dev;

		/*这次的数据保存用于差分*/
		// last_t265_x = t265_posx.data_f;
		// last_t265_y = t265_posy.data_f;
		// last_t265_z = t265_posz.data_f;

		// static float alpha = 0.1f;
		// t265_x_lpf += (t265_x_dev - t265_x_lpf) * alpha;
		// t265_y_lpf += (t265_y_dev - t265_y_lpf) * alpha;
		// t265_z_lpf += (t265_z_dev - t265_z_lpf) * alpha;

		// 跟踪微分器
		// x
		//  static float dT_s = 0.0f;
		//  static float x1_x = 0.0f, x2_x = 0.0f,v0_x = 0.0f;
		//  static float rx = 100.0f;

		// dT_s = t265_dt * 10.0f;
		// x2_x += (-2*rx*x2_x - rx*rx*(x1_x - t265_x_dev))*dT_s;
		// x1_x += x2_x*dT_s;
		// //y
		// static float x1_y = 0.0f, x2_y = 0.0f,v0_y = 0.0f;
		// static float ry = 100.0f;

		// x2_y += (-2*ry*x2_y - ry*ry*(x1_y - t265_y_dev))*dT_s;
		// x1_y += x2_y*dT_s;
		// //z
		// static float x1_z = 0.0f, x2_z = 0.0f,v0_z = 0.0f;
		// static float rz = 100.0f;

		// x2_z += (-2*rz*x2_z - rz*rz*(x1_z - t265_z_dev))*dT_s;
		// x1_z += x2_z*dT_s;

		// t265_x_lpf = x1_x;
		// t265_y_lpf = x1_y;
		// t265_z_lpf = x1_z;
	}
}

_dt_rx_anl_st rx_anl[3];
//===========================================================
// 数据每接收一字节，调用本函数，进行数据解析
//===========================================================
void MYDTRxOneByte(u8 type, u8 data)
{
	// static _dt_rx_anl_st rx_anl[3];

	switch (rx_anl[type].rxstate)
	{
	case 0:
	{
		if (data == 0xAA)
		{
			rx_anl[type].rxstate = 1;
			rx_anl[type].DT_RxBuffer[0] = data;
		}
	}
	break;
	case 1:
	{
		if ((data == HW_TYPE || data == HW_ALL || data == 0x05))
		{
			rx_anl[type].rxstate = 2;
			rx_anl[type].DT_RxBuffer[1] = data;
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 2:
	{
		rx_anl[type].rxstate = 3;
		rx_anl[type].DT_RxBuffer[2] = data;
	}
	break;
	case 3:
	{
		if (data < (DT_RX_BUFNUM - 5))
		{
			rx_anl[type].rxstate = 4;
			rx_anl[type].DT_RxBuffer[3] = data;
			rx_anl[type]._data_len = data;
			rx_anl[type]._data_cnt = 0;
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 4:
	{
		if (rx_anl[type]._data_len > 0)
		{
			rx_anl[type]._data_len--;
			rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt++] = data;
			if (rx_anl[type]._data_len == 0)
			{
				rx_anl[type].rxstate = 5;
			}
		}
		else
		{
			rx_anl[type].rxstate = 0;
		}
	}
	break;
	case 5:
	{
		rx_anl[type].rxstate = 6;
		rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt++] = data;
	}
	break;
	case 6:
	{
		u8 _data_cnt;
		rx_anl[type].rxstate = 0;
		rx_anl[type].DT_RxBuffer[4 + rx_anl[type]._data_cnt] = data;
		_data_cnt = rx_anl[type]._data_cnt + 5;
		// ano_dt_data_ok = 1;
		MYDTDataAnl(type, rx_anl[type].DT_RxBuffer, _data_cnt);
	}
	break;
	default:
	{
		rx_anl[type].rxstate = 0;
	}
	break;
	}
}

u8 DataTosend__[60];
/*
灵活帧共10帧可选，帧ID从0xF1到0xFA，每一帧最多可以携带10个数据。
数据类型可设为U8、S16、U16、S32。若要显示float，就根据精度乘 1000 or 100 变成 S32 型整数传输。
*/
/*ID：0xF1--0xFA：灵活帧*/
void ANODT_UserDefined_t265(s32 a, s32 b, s32 c) // 形参数据类型、形参个数可自定义，上位机对应配置即可。
{
	u8 _vcnt = 0;

	DataTosend__[_vcnt++] = 0xAA; // 帧头
	DataTosend__[_vcnt++] = 0xFF; // 目标地址
	DataTosend__[_vcnt++] = 0xF1; // 灵活帧ID范围:0xF1--0xFA，这里随便选个选个F1
	DataTosend__[_vcnt++] = 0;	  // 数据区长度

	DataTosend__[_vcnt++] = BYTE0(a); // 发送两次单字节a（由低到高）
	DataTosend__[_vcnt++] = BYTE1(a);
	DataTosend__[_vcnt++] = BYTE2(a);
	DataTosend__[_vcnt++] = BYTE3(a);

	DataTosend__[_vcnt++] = BYTE0(b); // 发送两次单字节b
	DataTosend__[_vcnt++] = BYTE1(b);
	DataTosend__[_vcnt++] = BYTE2(b);
	DataTosend__[_vcnt++] = BYTE3(b);

	DataTosend__[_vcnt++] = BYTE0(c); // 发送四次单字节c
	DataTosend__[_vcnt++] = BYTE1(c);
	DataTosend__[_vcnt++] = BYTE2(c);
	DataTosend__[_vcnt++] = BYTE3(c);

	DataTosend__[3] = _vcnt - 4; // 重新计算数据区长度

	u8 sc = 0, ac = 0;							 // 点击魔法棒-> 点击C/C++ -> 勾选C99 Mode 并在Misc Controls栏后输入"--c99"（双引号里面的内容）
	for (u8 i = 0; i < DataTosend__[3] + 4; i++) // 校验计算
	{
		sc += DataTosend__[i]; // 和校验计算
		ac += sc;			   // 附加校验计算
	}

	DataTosend__[_vcnt++] = sc; // 校验同样发送出去用于接收方对比校验
	DataTosend__[_vcnt++] = ac;

	Uart5_Send(DataTosend__, _vcnt);
}

void ANODT_UserDefined_t265_lpf(s32 a, s32 b, s32 c) // 形参数据类型、形参个数可自定义，上位机对应配置即可。
{
	u8 _vcnt = 0;

	DataTosend__[_vcnt++] = 0xAA; // 帧头
	DataTosend__[_vcnt++] = 0xFF; // 目标地址
	DataTosend__[_vcnt++] = 0xF3; // 灵活帧ID范围:0xF1--0xFA，这里随便选个选个F1
	DataTosend__[_vcnt++] = 0;	  // 数据区长度

	DataTosend__[_vcnt++] = BYTE0(a); // 发送两次单字节a（由低到高）
	DataTosend__[_vcnt++] = BYTE1(a);
	DataTosend__[_vcnt++] = BYTE2(a);
	DataTosend__[_vcnt++] = BYTE3(a);

	DataTosend__[_vcnt++] = BYTE0(b); // 发送两次单字节b
	DataTosend__[_vcnt++] = BYTE1(b);
	DataTosend__[_vcnt++] = BYTE2(b);
	DataTosend__[_vcnt++] = BYTE3(b);

	DataTosend__[_vcnt++] = BYTE0(c); // 发送四次单字节c
	DataTosend__[_vcnt++] = BYTE1(c);
	DataTosend__[_vcnt++] = BYTE2(c);
	DataTosend__[_vcnt++] = BYTE3(c);

	DataTosend__[3] = _vcnt - 4; // 重新计算数据区长度

	u8 sc = 0, ac = 0;							 // 点击魔法棒-> 点击C/C++ -> 勾选C99 Mode 并在Misc Controls栏后输入"--c99"（双引号里面的内容）
	for (u8 i = 0; i < DataTosend__[3] + 4; i++) // 校验计算
	{
		sc += DataTosend__[i]; // 和校验计算
		ac += sc;			   // 附加校验计算
	}

	DataTosend__[_vcnt++] = sc; // 校验同样发送出去用于接收方对比校验
	DataTosend__[_vcnt++] = ac;

	Uart5_Send(DataTosend__, _vcnt);
}

void ANODT_UserDefined_flow(s32 a, s32 b, s32 c, s32 d) // 形参数据类型、形参个数可自定义，上位机对应配置即可。
{
	u8 _vcnt = 0;

	DataTosend__[_vcnt++] = 0xAA; // 帧头
	DataTosend__[_vcnt++] = 0xFF; // 目标地址
	DataTosend__[_vcnt++] = 0xF2; // 灵活帧ID范围:0xF1--0xFA，这里随便选个选个F1
	DataTosend__[_vcnt++] = 0;	  // 数据区长度

	DataTosend__[_vcnt++] = BYTE0(a); // 发送两次单字节a（由低到高）
	DataTosend__[_vcnt++] = BYTE1(a);
	DataTosend__[_vcnt++] = BYTE2(a);
	DataTosend__[_vcnt++] = BYTE3(a);

	DataTosend__[_vcnt++] = BYTE0(b); // 发送两次单字节b
	DataTosend__[_vcnt++] = BYTE1(b);
	DataTosend__[_vcnt++] = BYTE2(b);
	DataTosend__[_vcnt++] = BYTE3(b);

	DataTosend__[_vcnt++] = BYTE0(c); // 发送四次单字节c
	DataTosend__[_vcnt++] = BYTE1(c);
	DataTosend__[_vcnt++] = BYTE2(c);
	DataTosend__[_vcnt++] = BYTE3(c);

	DataTosend__[_vcnt++] = BYTE0(d); // 发送四次单字节c
	DataTosend__[_vcnt++] = BYTE1(d);
	DataTosend__[_vcnt++] = BYTE2(d);
	DataTosend__[_vcnt++] = BYTE3(d);

	DataTosend__[3] = _vcnt - 4; // 重新计算数据区长度

	u8 sc = 0, ac = 0;							 // 点击魔法棒-> 点击C/C++ -> 勾选C99 Mode 并在Misc Controls栏后输入"--c99"（双引号里面的内容）
	for (u8 i = 0; i < DataTosend__[3] + 4; i++) // 校验计算
	{
		sc += DataTosend__[i]; // 和校验计算
		ac += sc;			   // 附加校验计算
	}

	DataTosend__[_vcnt++] = sc; // 校验同样发送出去用于接收方对比校验
	DataTosend__[_vcnt++] = ac;

	Uart5_Send(DataTosend__, _vcnt);
}

void ANODT_UserDefined_to_car_xy(s32 a, s32 b, s32 c) // 形参数据类型、形参个数可自定义，上位机对应配置即可。
{
	u8 _vcnt = 0;

	DataTosend__[_vcnt++] = 0xAA; // 帧头
	DataTosend__[_vcnt++] = 0xFF; // 目标地址
	DataTosend__[_vcnt++] = 0xF9; // 灵活帧ID范围:0xF1--0xFA，这里随便选个选个F1
	DataTosend__[_vcnt++] = 0;	  // 数据区长度

	DataTosend__[_vcnt++] = BYTE0(a); // 发送两次单字节a（由低到高）
	DataTosend__[_vcnt++] = BYTE1(a);
	DataTosend__[_vcnt++] = BYTE2(a);
	DataTosend__[_vcnt++] = BYTE3(a);

	DataTosend__[_vcnt++] = BYTE0(b); // 发送两次单字节b
	DataTosend__[_vcnt++] = BYTE1(b);
	DataTosend__[_vcnt++] = BYTE2(b);
	DataTosend__[_vcnt++] = BYTE3(b);

	DataTosend__[_vcnt++] = BYTE0(c); // 发送四次单字节c
	DataTosend__[_vcnt++] = BYTE1(c);
	DataTosend__[_vcnt++] = BYTE2(c);
	DataTosend__[_vcnt++] = BYTE3(c);

	DataTosend__[3] = _vcnt - 4; // 重新计算数据区长度

	u8 sc = 0, ac = 0;							 // 点击魔法棒-> 点击C/C++ -> 勾选C99 Mode 并在Misc Controls栏后输入"--c99"（双引号里面的内容）
	for (u8 i = 0; i < DataTosend__[3] + 4; i++) // 校验计算
	{
		sc += DataTosend__[i]; // 和校验计算
		ac += sc;			   // 附加校验计算
	}

	DataTosend__[_vcnt++] = sc; // 校验同样发送出去用于接收方对比校验
	DataTosend__[_vcnt++] = ac;

	Usart3_Send(DataTosend__, _vcnt);
}

// u8 DataToReceive__[100];
// /*数据解析*/
// void ANO_Anl(void)
// {
// 	u8 sc = 0, ac = 0;

// 	for (u8 i = 0; i < DataToReceive__[3] + 4; i++) // 接收方计算校验字节
// 	{
// 		sc += DataToReceive__[i];
// 		ac += sc;
// 	}
// 	// 接受方与发送方进行校验字节对比，不同代表通信出错，return退出函数。
// 	if ((sc != DataToReceive__[DataToReceive__[3] + 4]) || (ac != DataToReceive__[DataToReceive__[3] + 5]))
// 		return;

// 	// 开始判断功能码ID
// 	/*ID：0xE1：参数读取帧*/
// 	if (DataToReceive__[2] == 0X32)
// 	{
// 		for (u8 i = 0; i < 4; i++)
// 		{
// 			t265_posx.data_u[i] = *(DataToReceive__ + 4 + i);
// 		}
// 		for (u8 i = 0; i < 4; i++)
// 		{
// 			t265_posy.data_u[i] = *(DataToReceive__ + 8 + i);
// 		}
// 		for (u8 i = 0; i < 4; i++)
// 		{
// 			t265_posz.data_u[i] = *(DataToReceive__ + 12 + i);
// 		}
// 	}
// }

// /****接收方函数****/
// void Rcceive_dataGet(u8 data__)
// {
// 	static u8 sta__ = 0; // 状态量
// 	static u8 len__ = 0; // 数据区字节长度
// 	static u8 cnt__ = 0; // 当前已接收长度

// 	if (sta__ == 0) /*帧头*/
// 	{
// 		DataToReceive__[0] = data__;
// 		len__ = 0;
// 		cnt__ = 0; // 每重开一帧先清零len和cnt，否则会导致接收方校验出错。
// 		if (data__ == 0xAA)
// 			sta__ = 1;
// 	}
// 	else if (sta__ == 1) /*目标地址*/
// 	{
// 		DataToReceive__[1] = data__;
// 		sta__ = 2;
// 	}
// 	else if (sta__ == 2) /*帧ID*/
// 	{
// 		DataToReceive__[2] = data__;
// 		sta__ = 3;
// 	}
// 	else if (sta__ == 3) /*数据长度*/
// 	{
// 		if (data__ > 50)
// 			sta__ = 0; // 长度大于50代表错误，小于则正确
// 		else
// 		{
// 			DataToReceive__[3] = data__;
// 			len__ = data__; // 保存长度
// 			sta__ = 4;
// 		}
// 	}
// 	else if (sta__ == 4) /*数据内容*/
// 	{
// 		DataToReceive__[4 + cnt__++] = data__; // 开始接收数据区，接收长度大于字长代表接收完毕
// 		if (cnt__ >= len__)
// 			sta__ = 5;
// 	}
// 	else if (sta__ == 5) /*和校验*/
// 	{
// 		DataToReceive__[4 + cnt__++] = data__;
// 		sta__ = 6;
// 	}
// 	else if (sta__ == 6) /*附加校验*/
// 	{
// 		DataToReceive__[4 + cnt__++] = data__;
// 		sta__ = 0;

// 		ANO_Anl(); // 执行解析函数
// 	}
// }
